/**
 * @file ThetaStarPlanner.h
 * @brief Theta*算法路径规划器
 * @details
 * Theta*算法原理：
 * 1. 基于A*算法，但允许路径点之间进行视线检查
 * 2. 在扩展节点时，检查当前节点与祖父节点之间是否有直接视线
 * 3. 如果有直接视线，则跳过父节点直接连接到祖父节点
 * 4. 这样可以生成更短、更平滑的路径，避免网格锯齿效应
 *
 * 算法步骤：
 * 1. 初始化开放列表和关闭列表
 * 2. 将起点加入开放列表
 * 3. 循环直到找到目标或开放列表为空：
 *    a. 从开放列表取出f值最小的节点
 *    b. 如果是目标节点，则重建路径
 *    c. 将当前节点加入关闭列表
 *    d. 遍历邻居节点：
 *       - 如果邻居在关闭列表中，跳过
 *       - 计算从起点经过当前节点到邻居的g值
 *       - 如果新g值更小，更新邻居节点
 *       - 进行视线检查：如果当前节点的祖父节点与邻居有视线，直接连接
 * 4. 返回路径
 */

#ifndef THETASTAR_PLANNER_H
#define THETASTAR_PLANNER_H

#include "core/PlannerBase.h"
#include <queue>
#include <unordered_map>
#include <cmath>

namespace PathPlanning
{
/**
 * @brief Theta*算法配置参数
 */
struct ThetaStarConfig : public PlannerConfig
{
    bool        eightConnected    = true; // 使用8方向连接
    double      obstacleThreshold = 128.0;
    std::string heuristicType     = "euclidean"; // 启发式函数类型
    double      weight            = 1.0;         // 启发式权重

    // 视线检查参数
    int lineOfSightResolution = 10; // 视线检查分辨率

    ThetaStarConfig()
    {
        plannerName = "ThetaStar";
    }
};

/**
 * @brief Theta*搜索节点
 */
struct ThetaStarNode : public Node
{
    GridCell gridCell;

    ThetaStarNode(const GridCell& cell, const Point& worldPos) : gridCell(cell)
    {
        position = worldPos;
    }
};

/**
 * @brief Theta*节点比较器
 */
struct ThetaStarNodeCompare
{
    bool operator()(const std::shared_ptr<ThetaStarNode>& a, const std::shared_ptr<ThetaStarNode>& b) const
    {
        return a->fCost > b->fCost; // 最小堆
    }
};

class ThetaStarPlanner : public PlannerBase
{
public:
    ThetaStarPlanner(std::shared_ptr<Map> map);

    PlanningResult plan(const Point& start, const Point& goal) override;

private:
    /**
     * @brief 计算启发式函数值
     */
    double calculateHeuristic(const Point& from, const Point& to);

    /**
     * @brief 检查两点之间是否有直接视线
     */
    bool hasLineOfSight(const GridCell& a, const GridCell& b);

    /**
     * @brief 从网格坐标重建路径
     */
    std::vector<Pose>
    reconstructPath(const std::unordered_map<GridCell, std::shared_ptr<ThetaStarNode>, GridCellHash>& cameFrom,
                    const std::shared_ptr<ThetaStarNode>&                                             endNode);

    /**
     * @brief 更新顶点 - Theta*的核心操作
     */
    void updateVertex(const std::shared_ptr<ThetaStarNode>& current, const std::shared_ptr<ThetaStarNode>& neighbor,
                      const std::shared_ptr<ThetaStarNode>&                                       goalNode,
                      std::priority_queue<std::shared_ptr<ThetaStarNode>, std::vector<std::shared_ptr<ThetaStarNode>>,
                                          ThetaStarNodeCompare>&                                  openSet,
                      std::unordered_map<GridCell, std::shared_ptr<ThetaStarNode>, GridCellHash>& allNodes);
};

} // namespace PathPlanning

#endif